import steer
import vector


class Arrive (steer.Steer):
    
    def __init__(self):
        steer.Steer.__init__(self)

    
    def calculate(self, targetPosition):
        toTarget= targetPosition.subtract(self.position)
        dist = toTarget.length()
        if dist==0.0:
            return vector.Vector(0.0,0.0)                
        elif dist<50.0:
            decelRate = 1000.0
        elif dist<75.0:
                decelRate = 1.5
        elif dist<100.0:
                decelRate = 1.0
        elif dist>=100.0:
                decelRate = 0.2
        
        
        speed = dist/decelRate
        speed = min(speed,self.max_speed)
        #print speed    
        desiredVelocity=toTarget.divide(dist)
        desiredVelocity = desiredVelocity.multiple(speed)
            
        #return desiredVelocity
        
        # F = ( (V1 - V2).cutTo(maxV) * mass * czasJednostkowy ).cutTo(maxF)
        return desiredVelocity.subtract(self.velocity).cutTo(self.max_speed).multiple(self.mass).cutTo(self.max_force)
        